
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode.h
  * @author     baiyang
  * @date       2021-8-12
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "takeoff.h"
#include <arming/gp_arming.h>
#include <motor/gp_motors_matrix.h>
#include <mc_attitude_control/mc_attitude_control.h>
#include <mc_position_control/mc_position_control.h>
#include <rc_channel/rc_channel.h>
#include <common/location/location.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
// Auto Pilot Modes enumeration
typedef enum {
    STABILIZE =     0,  // manual airframe angle with manual throttle
    ACRO =          1,  // manual body-frame angular rate with manual throttle
    ALT_HOLD =      2,  // manual airframe angle with automatic throttle
    AUTO =          3,  // fully automatic waypoint control using mission commands
    GUIDED =        4,  // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
    LOITER =        5,  // automatic horizontal acceleration with automatic throttle
    RTL =           6,  // automatic return to launching point
    CIRCLE =        7,  // automatic circular flight with automatic throttle
    LAND =          9,  // automatic landing with horizontal position control
    DRIFT =        11,  // semi-autonomous position, yaw and throttle control
    SPORT =        13,  // manual earth-frame angular rate control with manual throttle
    FLIP =         14,  // automatically flip the vehicle on the roll axis
    AUTOTUNE =     15,  // automatically tune the vehicle's roll and pitch gains
    POSHOLD =      16,  // automatic position hold with manual override, with automatic throttle
    BRAKE =        17,  // full-brake using inertial/GPS system, no pilot input
    THROW =        18,  // throw to launch mode using inertial/GPS system, no pilot input
    AVOID_ADSB =   19,  // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
    GUIDED_NOGPS = 20,  // guided mode but only accepts attitude and altitude
    SMART_RTL =    21,  // SMART_RTL returns to home by retracing its steps
    FLOWHOLD  =    22,  // FLOWHOLD holds position with optical flow without rangefinder
    FOLLOW    =    23,  // follow attempts to follow another vehicle or ground station
    ZIGZAG    =    24,  // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
    SYSTEMID  =    25,  // System ID mode produces automated system identification signals in the controllers
    AUTOROTATE =   26,  // Autonomous autorotation
    AUTO_RTL =     27,  // Auto RTL, this is not a true mode, AUTO will report as this mode if entered to perform a DO_LAND_START Landing sequence
    TURTLE =       28,  // Flip over after crash
} ModeNumber;

// Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport
typedef enum {
    AltHold_MotorStopped,
    AltHold_Takeoff,
    AltHold_Landed_Ground_Idle,
    AltHold_Landed_Pre_Takeoff,
    AltHold_Flying
} AltHoldModeState;

// Navigation Yaw control
typedef struct {
    // auto flight mode's yaw mode
    uint8_t _mode;

    // Yaw will point at this location if mode is set to AUTO_YAW_ROI
    Vector3f_t roi;

    // yaw used for YAW_FIXED yaw_mode
    float _fixed_yaw_offset_cd;

    // Deg/s we should turn
    float _fixed_yaw_slewrate_cds;

    // time of the last yaw update
    uint32_t _last_update_ms;

    // heading when in yaw_look_ahead_yaw
    float _look_ahead_yaw;

    // turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
    float _yaw_angle_cd;
    float _yaw_rate_cds;
} AutoYaw;

typedef struct
{
    // returns a unique number specific to this mode
    ModeNumber (*mode_number)(void);

    // child classes should override these methods
    bool (*init)(bool ignore_checks);

    void (*exit)(void);
    void (*run)(void);
    bool (*requires_GPS)(void);
    bool (*has_manual_throttle)(void);
    bool (*allows_arming)(ArmingMethod method);
    bool (*is_autopilot)(void);
    bool (*has_user_takeoff)(bool must_navigate);
    bool (*in_guided_mode)(void);
    bool (*logs_attitude)(void);
    bool (*allows_save_trim)(void);
    bool (*allows_autotune)(void);
    bool (*allows_flip)(void);

    // return a string for this flightmode
    const char *(*name)(void);
    const char *(*name4)(void);
    bool (*is_taking_off)(void);

    bool (*is_landing)(void);

    // mode requires terrain to be present to be functional
    bool (*requires_terrain_failsafe)(void);

    // functions for reporting to GCS
    bool (*get_wp)(Location *loc);
    int32_t (*wp_bearing)(void);
    uint32_t (*wp_distance)(void);
    float (*crosstrack_error)(void);

    // return expected input throttle setting to hover:
    float (*throttle_hover)(void);

    bool (*do_user_takeoff_start)(float takeoff_alt_cm);

    // convenience references to avoid code churn in conversion:
    Position_ctrl *pos_control;
    Attitude_ctrl *attitude_control;
    MotorsMat_HandleTypeDef *motors;
    RCs_HandleTypeDef *channel_roll;
    RCs_HandleTypeDef *channel_pitch;
    RCs_HandleTypeDef *channel_throttle;
    RCs_HandleTypeDef *channel_yaw;
    float G_Dt;

    // altitude above-ekf-origin below which auto takeoff does not control horizontal position
    bool *auto_takeoff_no_nav_active;
    float *auto_takeoff_no_nav_alt_cm;

    UserTakeOff *takeoff;
    AutoYaw *auto_yaw;
} Mode;

typedef struct {
    Mode mode;
} ModeStabilize;

typedef struct {
    Mode mode;
} ModeAltHold;

/*----------------------------------variable----------------------------------*/
extern Mode mode_base;
extern ModeStabilize mode_stabilize;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mode_ctor();

bool mode_do_user_takeoff(Mode* mode, float takeoff_alt_cm, bool must_navigate);
int32_t mode_get_alt_above_ground_cm(Mode* mode);

static inline Vector3f_t mode_get_vel_desired_cms(Mode* mode) {
    // note that position control isn't used in every mode, so
    // this may return bogus data:
    return posctrl_get_vel_desired_cms(mode->pos_control);
}

// helper functions
bool mode_is_disarmed_or_landed();
void mode_zero_throttle_and_relax_ac(bool spool_up); // spool_up,default false
void mode_zero_throttle_and_hold_attitude();
void mode_make_safe_spool_down(Mode* mode);

// functions to control landing
// in modes that support landing
void mode_land_run_horizontal_control(Mode* mode);
void mode_land_run_vertical_control(Mode* mode, bool pause_descent); // pause_descent,default false

AltHoldModeState mode_get_alt_hold_state(float target_climb_rate_cms);

// method shared by both Guided and Auto for takeoff.  This is
// waypoint navigation but the user can control the yaw.
void mode_auto_takeoff_run(Mode* mode);
void mode_auto_takeoff_set_start_alt(Mode* mode);

// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.
void mode_update_simple_mode(Mode* mode);
bool mode_set_mode(Mode* mode, ModeNumber mode_number, ModeReason reason);
void mode_set_land_complete(Mode* mode, bool b);
void mode_set_throttle_takeoff();
uint16_t mode_get_pilot_speed_dn(Mode* mode);

/// ModeStabilize
Mode* get_mode_stabilize();
void mode_stabilize_ctor(Mode* mode);

/// ModeAltHold
Mode* get_mode_althold();
void mode_althold_ctor(Mode *mode);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



